#include "uav_swarms/communication_multirotor.hpp"
#include "yolo_detect_message/msg/move_interface.hpp"
#include "iostream"

using namespace std::chrono_literals;

bool Communication::TargetYawCalibration(const geographic_msgs::msg::GeoPoseStamped target_motion, double &yaw){
    // yaw = current_yaw_;

    double distance;
    // GPS control
    double d_lat = (target_motion.pose.position.latitude - current_latitude_) * M_PI /180.0;
    double d_lon = (target_motion.pose.position.longitude - current_longitude_) * M_PI /180.0;
    double lat1_rad = current_latitude_ * M_PI /180.0;
    // 
    double Eerth_radius = 6378137.0;
    double dx = Eerth_radius * d_lon * std::cos(lat1_rad);
    double dy = Eerth_radius * d_lat;
    // double dz = target_motion.pose.position.altitude - current_altitude_;

    distance = sqrt(dx*dx+dy*dy);
    if(distance < 0.8)
    {
        return false;
    }

    yaw = std::atan2(dy,dx);
    RCLCPP_INFO(this->get_logger(), "target yaw: %f", yaw);

    return (abs(current_yaw_ - yaw) > 0.2);
}

Communication::Communication()
    : Node("communication_multirotor_node"),
      current_yaw_(0),              // 初始化当前偏航角
      hover_flag_(0),               // 初始化悬停标志
      arm_state_(false),            // 初始化无人机解锁状态为锁定
      motion_type_(0) {  
                   // 初始化运动类型

        initialize_parameters();
        namespaces = namespaces + "/";
        initial_setup_ = true;
        {
            std::lock_guard<std::mutex> lock(coordinate_frame_mutex_);
            coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED;
        }
        {
            std::lock_guard<std::mutex> lock(position_mutex_);
            target_motion_ = construct_target(
                0,
                0,
                default_height_,
                0, 0, 0,
                0, 0, 0, 
                0 
            );
        }
        first_received_controller = false;
        // coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED;
        // ROS 订阅者：订阅无人机的局部位置信息（Pose），回调函数是 local_pose_callback
        init_flag_ = false;
        local_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            vehicle_type_ + "_" + vehicle_id_ + "/local_position/pose",rclcpp::QoS(100).best_effort() ,
            std::bind(&Communication::local_pose_callback, this, _1));
        
        global_pose_subscribers_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            vehicle_type_ + "_" + vehicle_id_ + "/global_position/global", rclcpp::QoS(10).best_effort(), std::bind(&Communication::global_position_callback, this, std::placeholders::_1));
     
        // ROS 订阅者：订阅命令字符串，回调函数是 cmd_callback
        cmd_sub_ = this->create_subscription<std_msgs::msg::String>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/cmd", rclcpp::QoS(100).best_effort(),
            std::bind(&Communication::cmd_callback, this, _1));
        
        // ROS 订阅者：订阅来自机体坐标系下的目标位置命令，回调函数是 cmd_pose_flu_callback
        cmd_pose_flu_sub_ = this->create_subscription<geometry_msgs::msg::Pose>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/cmd_pose_flu", 1,
            std::bind(&Communication::cmd_pose_flu_callback, this, _1));

        // ROS 订阅者：订阅无人机运动控制信息，回调函数是 move_interface_callback  所有的运动控制信息都发布到这个主题
        move_interface_ = this->create_subscription<yolo_detect_message::msg::MoveInterface>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/move", rclcpp::QoS(10).best_effort(),
            std::bind(&Communication::move_interface_callback, this, std::placeholders::_1)
        );
        cmd_pose_enu_sub_ = this->create_subscription<geometry_msgs::msg::Pose>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/cmd_pose_enu", 1,
            std::bind(&Communication::cmd_pose_enu_callback, this, std::placeholders::_1)
        );

        // Subscribe to FLU coordinate frame velocity information
        cmd_vel_flu_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/cmd_vel_flu", 1,
            std::bind(&Communication::cmd_vel_flu_callback, this, std::placeholders::_1)
        );

        // Subscribe to ENU coordinate frame velocity information
        cmd_vel_enu_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/cmd_vel_enu", 1,
            std::bind(&Communication::cmd_vel_enu_callback, this, std::placeholders::_1)
        );

        // Subscribe to FLU coordinate frame acceleration information
        cmd_accel_flu_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/cmd_accel_flu", 1,
            std::bind(&Communication::cmd_accel_flu_callback, this, std::placeholders::_1)
        );

        // Subscribe to ENU coordinate frame acceleration information
        cmd_accel_enu_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/cmd_accel_enu", 1,
            std::bind(&Communication::cmd_accel_enu_callback, this, std::placeholders::_1)
        );

        cmd_gps_move_sub_  = this->create_subscription<yolo_detect_message::msg::Geolocation>(
            namespaces + vehicle_type_ + "_" + vehicle_id_ + "/gps_move", 1,
            std::bind(&Communication::gps_callback, this, std::placeholders::_1)
        );

        // ROS 订阅者：订阅无人机状态信息，回调函数是 state_callback
        state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
            vehicle_type_ + "_" + vehicle_id_ +"/state", rclcpp::QoS(10).best_effort(), std::bind(&Communication::state_callback, this, std::placeholders::_1));

        // ROS 发布者：发布目标运动状态到无人机位置控制的主题
        target_motion_pub_ = this->create_publisher<mavros_msgs::msg::PositionTarget>(
            vehicle_type_ + "_" + vehicle_id_ + "/setpoint_raw/local", 1);

        gps_publisher_controler_ = this->create_publisher<geographic_msgs::msg::GeoPoseStamped>(
            vehicle_type_ + "_" + vehicle_id_ + "/setpoint_position/global", 1);

        // ROS 服务客户端：用于与 MAVROS 交互，控制无人机的解锁（arming）状态
        arm_client_ = this->create_client<mavros_msgs::srv::CommandBool>(
            vehicle_type_ + "_" + vehicle_id_ + "/cmd/arming");
        
        // ROS 服务客户端：用于与 MAVROS 交互，切换无人机的飞行模式
        flight_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>(
            vehicle_type_ + "_" + vehicle_id_ + "/set_mode");
            
        
        // 启动该服务的话题
        service_status_sub_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(
            namespaces + vehicle_type_+ "_" + vehicle_id_ +"_communication_" + "server_topic", rclcpp::QoS(10).best_effort(), std::bind(&Communication::service_status_callback, this, std::placeholders::_1));

        
        // 打印初始化完成的信息到日志
        RCLCPP_INFO(this->get_logger(), "%s: communication initialized", (vehicle_type_ + "_" + vehicle_id_).c_str());
        
        // 是否已经获取了初始化的位姿
        init_received_position = false;
        control_type = false;
        // 是否到达第一个初始位置
        first_arrived == false;

        last_cmd_ = "OFFBOARD";

        publish_timer = this->create_wall_timer(
            100ms, 
            std::bind(&Communication::publish_target, this) 
        );
        // init_offboard_and_arm();
    }
    double Communication::q2yaw(const geometry_msgs::msg::Quaternion &q) {
        tf2::Quaternion quat(q.x, q.y, q.z, q.w);
        tf2::Matrix3x3 mat(quat);
        double roll, pitch, yaw;
        mat.getRPY(roll, pitch, yaw);
        return yaw;
    }

    mavros_msgs::msg::PositionTarget Communication::construct_target(
    double x, double y, double z, double vx, double vy, double vz,
    double afx, double afy, double afz, double yaw, double yaw_rate) {

    

    mavros_msgs::msg::PositionTarget target_raw_pose;
    // 设置时间戳
    target_raw_pose.header.stamp = this->get_clock()->now();
    target_raw_pose.header.frame_id = "map";

    // 设置坐标系
    target_raw_pose.coordinate_frame = coordinate_frame_;
   

    // 设置位置
    target_raw_pose.position.x = x;
    target_raw_pose.position.y = y;
    target_raw_pose.position.z = z;

    // 设置速度
    target_raw_pose.velocity.x = vx;
    target_raw_pose.velocity.y = vy;
    target_raw_pose.velocity.z = vz;

    // 设置加速度或力
    target_raw_pose.acceleration_or_force.x = afx;
    target_raw_pose.acceleration_or_force.y = afy;
    target_raw_pose.acceleration_or_force.z = afz;

    // 设置偏航角度和偏航速率
    target_raw_pose.yaw = yaw;
    target_raw_pose.yaw_rate = yaw_rate;

    // 根据 motion_type 设置类型掩码
    if (motion_type_ == 0) {
        target_raw_pose.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_VX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_VY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_VZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_YAW_RATE;
    } else if (motion_type_ == 1) {
        target_raw_pose.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_PX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_PY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_PZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_YAW;
    } else if (motion_type_ == 2) {
        target_raw_pose.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_PX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_PY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_PZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_VX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_VY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_VZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_YAW;
    }else if (motion_type_ == 3) {
        target_raw_pose.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_PX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_PY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_VZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFX |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFY |
                                    mavros_msgs::msg::PositionTarget::IGNORE_AFZ |
                                    mavros_msgs::msg::PositionTarget::IGNORE_YAW;
    }

    return target_raw_pose;
}


void Communication::cmd_pose_flu_callback(const geometry_msgs::msg::Pose::SharedPtr msg) {
    first_received_controller = true;
    // RCLCPP_INFO(this->get_logger(), "cmd_pose_flu_callback,received msg x:%f,y:%f,z:%f",msg->position.x,msg->position.y,msg->position.z);
    // 设置坐标系为 9 (FLU)
    {
        std::lock_guard<std::mutex> lock(coordinate_frame_mutex_);
        coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_BODY_OFFSET_NED;
    }

    {   
        std::lock_guard<std::mutex> lock(position_mutex_ );
        motion_type_ = 0;

        // 从四元数中提取 yaw 值
        double yaw = q2yaw(msg->orientation);

        // 构建目标位置并将其赋值给 target_motion_
        target_motion_ = construct_target(
            msg->position.x,
            msg->position.y,
            msg->position.z,
            0, 0, 0,
            0, 0, 0, 
            yaw 
        );
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = false;
    }
}


void Communication::cmd_pose_enu_callback(const geometry_msgs::msg::Pose::SharedPtr msg) {

    first_received_controller = true;
    // RCLCPP_INFO(this->get_logger(), "cmd_pose_enu_callback,received msg x:%f,y:%f,z:%f",msg->position.x,msg->position.y,msg->position.z);
    {
        std::lock_guard<std::mutex> lock(coordinate_frame_mutex_);
        coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED;
        
    }
    {
        std::lock_guard<std::mutex> lock(position_mutex_ );
        motion_type_ = 0;
        RCLCPP_INFO(this->get_logger(), "cmd_pose_enu_callback,received msg x:%f,y:%f,z:%f",msg->position.x,msg->position.y,msg->position.z);
        // 从四元数提取 yaw
        double yaw = q2yaw(msg->orientation);

        // 构建目标位置消息
        target_motion_ = construct_target(msg->position.x, msg->position.y, msg->position.z, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, yaw);
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = false;
    }
}

void Communication::cmd_vel_flu_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    // RCLCPP_INFO(this->get_logger(), "cmd_vel_flu_callback,received msg x:%f,y:%f,z:%f",msg->linear.x,msg->linear.y,msg->linear.z);
    first_received_controller = true;
    {
        std::lock_guard<std::mutex> lock(coordinate_frame_mutex_);
        coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_BODY_NED  ;
    }
    {
        std::lock_guard<std::mutex> lock(position_mutex_ );
        motion_type_ = 1;

        // 构建速度控制目标
        target_motion_ = construct_target(
            0, 0, 0,              // 位置为空
            msg->linear.x,         // 速度 x
            msg->linear.y,         // 速度 y
            msg->linear.z,         // 速度 z
            0, 0, 0,              // 加速度为空
            0,                    // Yaw 为空
            msg->angular.z         // Yaw 速率
        );
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = false;
    }
    
}


void Communication::cmd_vel_enu_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    first_received_controller = true;
    {
        std::lock_guard<std::mutex> lock(coordinate_frame_mutex_);
        coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED;
        
    }
    {
        std::lock_guard<std::mutex> lock(position_mutex_ );
        motion_type_ = 1;

        // 构建速度控制目标
        target_motion_ = construct_target(
            0, 0, 0,               // 位置为空
            msg->linear.x,          // 速度 x
            msg->linear.y,          // 速度 y
            msg->linear.z,          // 速度 z
            0, 0, 0,               // 加速度为空
            0,                     // Yaw 为空
            msg->angular.z          // Yaw 速率
        );
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = false;
    }
}


void Communication::cmd_accel_flu_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    // 判断是否进入悬停状态
    first_received_controller = true;
    hover_state_transition(msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.z);

    // 如果不处于悬停状态，更新目标加速度
    if (hover_flag_ == 0) {
        // 设置 FLU 坐标系
        coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_BODY_NED;

        // 设置运动类型为 2，表示加速度控制
        motion_type_ = 2;

        // 构建加速度控制目标
        target_motion_ = construct_target(
            0, 0, 0,                // 位置为空
            0, 0, 0,                // 速度为空
            msg->linear.x,           // 加速度 x
            msg->linear.y,           // 加速度 y
            msg->linear.z,           // 加速度 z
            0,                       // Yaw 为空
            msg->angular.z           // Yaw 速率
        );
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = false;
    }
}



void Communication::cmd_accel_enu_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    first_received_controller = true;
    // 判断是否进入悬停状态
    hover_state_transition(msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.z);

    // 如果不处于悬停状态，更新目标加速度
    if (hover_flag_ == 0) {
        // 设置 ENU 坐标系
        coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED;

        // 设置运动类型为 2，表示加速度控制
        motion_type_ = 2;
        
        // 构建加速度控制目标
        target_motion_ = construct_target(
            0, 0, 0,                // 位置为空
            0, 0, 0,                // 速度为空
            msg->linear.x,           // 加速度 x
            msg->linear.y,           // 加速度 y
            msg->linear.z,           // 加速度 z
            0,                       // Yaw 为空
            msg->angular.z           // Yaw 速率
        );
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = false;
    }

}

void Communication::cmd_vel_fixed_high_callback(const geometry_msgs::msg::Twist::SharedPtr vel, const geometry_msgs::msg::Pose::SharedPtr pose, const bool is_body){
    
    first_received_controller = true;
    {
        std::lock_guard<std::mutex> lock(coordinate_frame_mutex_);
        if(is_body)
        {
            coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_BODY_NED  ;
        }
        else
        {
            coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED  ;
        }
    }
    {
        std::lock_guard<std::mutex> lock(position_mutex_ );
        motion_type_ = 3;

        // 构建速度控制目标
        target_motion_ = construct_target(
            0, 0, // 位置为空
            pose->position.z,              
            vel->linear.x,         // 速度 x
            vel->linear.y,         // 速度 y
            0,         // 速度 z
            0, 0, 0,              // 加速度为空
            0,                    // Yaw 为空
            vel->angular.z         // Yaw 速率
        );
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = false;
    }
    
}


void Communication::cmd_callback(const std_msgs::msg::String::SharedPtr msg) {
    if (service_status == false)
    {
        RCLCPP_ERROR(this->get_logger(), "Service not available");
        return;
    }
    RCLCPP_INFO(this->get_logger(), "%s_%s: %s", vehicle_type_.c_str(), vehicle_id_.c_str(), msg->data.c_str());
    // 如果消息与上一个命令相同，或者消息为空，或者为“停止控制”，则不处理
    if (msg->data.empty() || msg->data == "stop controlling") {
        return;
    }

    // 处理 ARM 命令
    else if (msg->data == "ARM") {
        arm_state_ = arm();
        RCLCPP_INFO(this->get_logger(), "%s_%s: Armed %s", vehicle_type_.c_str(), vehicle_id_.c_str(), arm_state_ ? "true" : "false");
    }

    // 处理 DISARM 命令
    else if (msg->data == "DISARM") {
        arm_state_ = !disarm();  // 如果 disarm 成功，arm_state_ 应该为 false
        RCLCPP_INFO(this->get_logger(), "%s_%s: Armed %s", vehicle_type_.c_str(), vehicle_id_.c_str(), arm_state_ ? "true" : "false");
    }

    // 处理 mission 命令（检查消息前缀是否为 "mission"）
    else if (msg->data.substr(0, 7) == "mission" && msg->data != mission) {
        mission = msg->data;
        RCLCPP_INFO(this->get_logger(), "%s_%s: %s", vehicle_type_.c_str(), vehicle_id_.c_str(), mission.c_str());
    }

    // 处理其他飞行模式命令
    else {
        flight_mode_ = msg->data;
        flight_mode_switch();
    }

    // 记录最后一次的命令
    last_cmd_ = msg->data;
}





void Communication::hover_state_transition(double x, double y, double z, double w) {
    // 如果 x, y, z 或 w 中的任意一个值超过指定阈值，退出悬停模式
    if (std::abs(x) > 0.02 || std::abs(y) > 0.02 || std::abs(z) > 0.02 || std::abs(w) > 0.005) {
        hover_flag_ = 0;
        flight_mode_ = "OFFBOARD";
    }
    // 如果未进入悬停模式，且满足悬停条件，则进入悬停模式
    else if (flight_mode_ != "HOVER") {
        hover_flag_ = 1;
        flight_mode_ = "HOVER";
        hover();  // 调用悬停函数
    }
}

void Communication::global_position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg){

    auto now = this->get_clock()->now();  // 获取当前时间

    last_recieve_global_p_ = now;

    current_latitude_ = msg->latitude;
    current_longitude_ = msg->longitude;
    current_altitude_ = msg->altitude;
}

void Communication::hover() {
    {
        std::lock_guard<std::mutex> lock(target_gps_position_mutex_);

        auto now = this->get_clock()->now();  // 获取当前时间
        
        rclcpp::Duration duration = now - last_recieve_global_p_; 

        RCLCPP_INFO(this->get_logger(), "rclcpp::Duration duration = now - last_recieve_global_p_; ");

        if(duration < rclcpp::Duration::from_seconds(0.5))
        {
            RCLCPP_INFO(this->get_logger(), "if(duration < rclcpp::Duration::from_seconds(1.0))");
            target_gps_position.header.stamp = now;
            if(!hover_pos_setted_)
            {

                hover_latitude_ = current_position_.position.x;
                hover_longitude_ = current_position_.position.y;
                hover_altitude_ = current_position_.position.z;
                // hover_latitude_ = current_latitude_;
                // hover_longitude_ = current_longitude_;
                // hover_altitude_ = current_altitude_;
                hover_pos_setted_ = true;
            }
            else
            {
                {
                    std::lock_guard<std::mutex> lock(coordinate_frame_mutex_);
                    coordinate_frame_ = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED;
                    
                }
                {
                    std::lock_guard<std::mutex> lock(position_mutex_ );
                    motion_type_ = 0;
                    // 构建目标位置消息
                    target_motion_ = construct_target(hover_latitude_, hover_longitude_, hover_altitude_, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, current_yaw_);
                }
                // target_gps_position.pose.position.longitude = hover_longitude_;
                // target_gps_position.pose.position.latitude = hover_latitude_;
                // target_gps_position.pose.position.altitude = hover_altitude_;
                RCLCPP_ERROR(this->get_logger(), "hover_longitude_ is : %f, hover_latitude_: %f, hover_altitude_: %f",hover_longitude_,hover_latitude_,hover_altitude_);
            }
            {
                std::lock_guard<std::mutex> lock(control_type_mutex);
                control_type = false;
                first_received_controller = true;
            }

        }
        else
        {
            std::shared_ptr<geometry_msgs::msg::Twist> vel;
            if (!vel) {
                vel = std::make_shared<geometry_msgs::msg::Twist>();
            }
            vel->linear.x = 0;
            vel->linear.y = 0;
            vel->linear.z = 0;
            vel->angular.x = 0;
            vel->angular.y = 0;
            vel->angular.z = 0;
            cmd_vel_flu_callback(vel);
        }
    }

    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }

    // // 打印当前飞行模式信息
    // RCLCPP_INFO(this->get_logger(), "%s: %s", (vehicle_type_ + "_" + vehicle_id_).c_str(), flight_mode_.c_str());
}

bool Communication::flight_mode_switch() {
    RCLCPP_INFO(this->get_logger(), "%s: %s", (vehicle_type_ + "_" + vehicle_id_).c_str(), flight_mode_.c_str());
    // 如果飞行模式是 HOVER，则进入悬停模式
    if (flight_mode_ == "HOVER") {
        hover_flag_ = 1;
        hover();  // 调用 hover 函数进入悬停模式
    }
    // 否则，调用飞行模式服务来切换模式
    else {
        // 构建服务请求
        auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
        request->custom_mode = flight_mode_;
        RCLCPP_INFO(this->get_logger(), "%s: %s", (vehicle_type_ + "_" + vehicle_id_).c_str(), flight_mode_.c_str());
        // 等待飞行模式服务可用
        if (flight_mode_client_->wait_for_service(std::chrono::seconds(2))) {
            // 调用飞行模式服务
            auto result = flight_mode_client_->async_send_request(request);
        } else {
            RCLCPP_ERROR(this->get_logger(), "Flight mode service not available");
            return false;
        }
    }
    return true;
}



bool Communication::arm() {
    // 创建服务请求
    auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
    request->value = true;  // true 表示武装（arm）

    // 等待 arm 服务可用
    if (arm_client_->wait_for_service(std::chrono::seconds(1))) {
        // 发送异步服务请求
        auto result = arm_client_->async_send_request(request);
        return true;  // 武装成功
    } else {
        RCLCPP_ERROR(this->get_logger(), "Arm service not available");
        return false;
    }
}

bool Communication::disarm() {
    // 创建服务请求
    auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
    request->value = false;  // false 表示解除武装（disarm）

    // 等待 arm 服务可用
    if (arm_client_->wait_for_service(std::chrono::seconds(1))) {
        // 发送异步服务请求
        auto result = arm_client_->async_send_request(request);
        return true;  // 解除武装成功
        
    } else {
        RCLCPP_ERROR(this->get_logger(), "disArm service not available");
        return false;
    }
    last_request_ = this->get_clock()->now();
}


void Communication::publish_target(){
    if (service_status == false || first_received_controller == false )
    {
        RCLCPP_ERROR(this->get_logger(), "Service not available");
        RCLCPP_ERROR(this->get_logger(), "service_status is : %d, init_received_position: %d",service_status,first_received_controller);
        return;
    }
    

    if (!current_state_.connected) {
            RCLCPP_WARN(this->get_logger(), "Waiting for FCU connection...");
            return;
    }

    if (current_state_.mode != last_cmd_ && (this->get_clock()->now() - last_request_ > rclcpp::Duration(5.0)))
    {
        RCLCPP_WARN(this->get_logger(), "call mode to %s ...",last_cmd_.c_str());
        if (last_cmd_ == "OFFBOARD" )
        {
            set_offboard_mode();
        }else{
            set_flight_mode(last_cmd_);
        }
        

        last_request_ = this->get_clock()->now();
        
    }else if (!current_state_.armed && (this->get_clock()->now() - last_request_ > rclcpp::Duration(5.0)))
    {
        if (last_cmd_ == "OFFBOARD")
        {
            RCLCPP_WARN(this->get_logger(), "Call Arming service ...");
            arm_drone();
        }
        
        last_request_ = this->get_clock()->now();
    }

    // auto now = this->get_clock()->now();  // 获取当前时间
    // rclcpp::Duration duration = now - last_request_; 
    // if (duration > rclcpp::Duration::from_seconds(1.0)  && last_cmd_ == "OFFBOARD")
    // {
    //     std::shared_ptr<geometry_msgs::msg::Twist> vel;
    //     if (!vel) {
    //         vel = std::make_shared<geometry_msgs::msg::Twist>();
    //     }
    //     vel->linear.x = 0;
    //     vel->linear.y = 0;
    //     vel->linear.z = 0;
    //     vel->angular.x = 0;
    //     vel->angular.y = 0;
    //     vel->angular.z = 0;
    //     cmd_vel_flu_callback(vel);
    // }
    
    // // 发布对应的控制信息到目标位置
    // if (control_type == false &&  last_cmd_ == "OFFBOARD")
    // {
    //     target_motion_.header.stamp = this->get_clock()->now();
    //     target_motion_pub_->publish(target_motion_);

    // }else if (control_type == true && last_cmd_ == "OFFBOARD")
    // {
    //     target_gps_position.header.stamp = this->get_clock()->now();
    //     gps_publisher_controler_->publish(target_gps_position);
    // }

    double yaw = current_yaw_;

    RCLCPP_WARN(this->get_logger(), "current yaw is  %f ...",yaw);

    auto now = this->get_clock()->now();  // 获取当前时间
    rclcpp::Duration duration = now - last_request_; 
    if (duration > rclcpp::Duration::from_seconds(1.0)  && last_cmd_ == "OFFBOARD")
    {
        hover();
    }
    else
    {
        hover_pos_setted_ = false;
    }
    
    // 发布对应的控制信息到目标位置
    if (control_type == false &&  last_cmd_ == "OFFBOARD")
    {
        target_motion_.header.stamp = this->get_clock()->now();
        
        target_motion_pub_->publish(target_motion_);

    }else if (control_type == true && last_cmd_ == "OFFBOARD")
    {
        target_gps_position.header.stamp = this->get_clock()->now();
        if(TargetYawCalibration(target_gps_position, yaw))
        {
            // mavros_msgs::msg::PositionTarget newTarget = ControlYaw(yaw);
            // target_motion_pub_->publish(newTarget);
            tf2::Quaternion tf2q;
            tf2q.setRPY(0,0,yaw);
            target_gps_position.pose.orientation.x = tf2q.x();
            target_gps_position.pose.orientation.y = tf2q.y();
            target_gps_position.pose.orientation.z = tf2q.z();
            target_gps_position.pose.orientation.w = tf2q.w();

            auto newTarget = target_gps_position;
            newTarget.pose.position.latitude = current_latitude_;
            newTarget.pose.position.longitude = current_longitude_;
            newTarget.pose.position.altitude = current_altitude_;

            gps_publisher_controler_->publish(newTarget);
        }
        else{
            gps_publisher_controler_->publish(target_gps_position);
        }
    }
}


void Communication::local_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg){
    
    current_locate = *msg;
    
    if (init_received_position == false)
    {
        origin_position = *msg;
        {
             std::lock_guard<std::mutex> lock(position_mutex_);
             target_motion_ = construct_target(
                origin_position.pose.position.x,
                origin_position.pose.position.y,
                origin_position.pose.position.z + default_height_,
                0, 0, 0,
                0, 0, 0, 
                0 
            );
            init_target_motion_.pose.position.x = origin_position.pose.position.x;
            init_target_motion_.pose.position.y = origin_position.pose.position.y;
            init_target_motion_.pose.position.z = origin_position.pose.position.z + default_height_;
        }

        RCLCPP_INFO(this->get_logger(),"init origin success !");
    }

    current_position_.position = msg->pose.position;
    current_position_.orientation = msg->pose.orientation;
    current_yaw_ = q2yaw(current_position_.orientation);
    {
        std::lock_guard<std::mutex> lockk(can_land_mutex);
        can_land = is_arrival(origin_position,current_locate,0.05);
    }
  
    if (first_arrived == false)
    {
        bool result = false;
        std::lock_guard<std::mutex> lock(first_arrived_mutex);
        result = is_arrival(init_target_motion_,*msg,1.0);
        if (result == true)
        {
            first_arrived = true;
        }
    }
}

void Communication::info_topic(){
    RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());
    RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", vehicle_type_.c_str());
   
    RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", vechile_nums);
}

void Communication::state_callback(const mavros_msgs::msg::State::SharedPtr msg) {
        current_state_ = *msg;
        if(!current_state_.armed){
            std::lock_guard<std::mutex> locker(init_received_position_mutxt_);
            init_received_position = false;
        }
        // RCLCPP_INFO(this->get_logger(),"this get  message");
}

void Communication::initialize_parameters() {
        this->declare_parameter<std::string>("vehicle_type", "drone");
        this->declare_parameter<std::string>("uav_namespace", "uav_swarms");
        this->declare_parameter<int>("vehicle_ids", 0);
        this->declare_parameter<int>("vehicle_num", 1);
        this->declare_parameter<double>("defautl_height", 35.0);
    
        this->get_parameter("vehicle_type", vehicle_type_);
        RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", vehicle_type_.c_str());

        std::string uav_namespace;
        this->get_parameter("uav_namespace", namespaces);
        RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());

        int vehicle_ids;
        if (this->get_parameter("vehicle_ids", vehicle_ids)) {
            RCLCPP_INFO(this->get_logger(), "Vehicle IDs: %d", vehicle_ids);
        } else {
            RCLCPP_WARN(this->get_logger(), "Parameter 'vehicle_ids' not found");
        }
        vehicle_id_ = std::to_string(vehicle_ids);

        int vehicle_num;
        this->get_parameter("vehicle_num", vehicle_num);
        RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", vehicle_num);

        double default_height;
        this->get_parameter("defautl_height", default_height_);
        RCLCPP_INFO(this->get_logger(), "defautl_height: %f", default_height_);

    }


     void Communication::init_offboard_and_arm()
    {
        // 等待服务可用
        while (!arm_client_->wait_for_service(std::chrono::seconds(1)) ||
               !flight_mode_client_->wait_for_service(std::chrono::seconds(1)))
        {
            RCLCPP_INFO(this->get_logger(), "Waiting for arming and mode change services...");        
        }

        // 发布初始位置多次以满足 OFFBOARD 模式切换条件
        for (int i = 0; i < 10; ++i)
        {
            this->publish_target();
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }

        // 设置模式为 OFFBOARD
        auto set_mode_request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
        set_mode_request->base_mode = 0;
        set_mode_request->custom_mode = "OFFBOARD";

        auto set_mode_result = flight_mode_client_->async_send_request(set_mode_request);
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), set_mode_result) ==
            rclcpp::FutureReturnCode::SUCCESS)
        {
            RCLCPP_INFO(this->get_logger(), "Set mode to OFFBOARD successfully.");
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to set mode to OFFBOARD.");
        }

        // 解锁无人机
        auto arm_request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
        arm_request->value = true;

        auto arm_result = arm_client_->async_send_request(arm_request);
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), arm_result) ==
            rclcpp::FutureReturnCode::SUCCESS)
        {
            RCLCPP_INFO(this->get_logger(), "Armed successfully.");
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to arm the drone.");
        }
    }


void Communication::arm_drone()
{
    if (!arm_client_->wait_for_service(std::chrono::seconds(1)))
    {
        RCLCPP_ERROR(this->get_logger(), "Arming service not available");
        return;
    }

    auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
    request->value = true;  // 发送ARM命令

    // 异步发送请求，并使用回调函数处理结果
    auto result_future = arm_client_->async_send_request(request,
        [this](rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedFuture result) {
            if (result.get()->success)
            {
                init_received_position = true;
                RCLCPP_INFO(this->get_logger(), "Drone armed successfully");
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to arm drone");
            }
        });
}

void Communication::set_offboard_mode()
{
    if (!flight_mode_client_->wait_for_service(std::chrono::seconds(1)))
    {
        RCLCPP_ERROR(this->get_logger(), "Set mode service not available");
        return;
    }

    auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    request->custom_mode = "OFFBOARD";

    // 异步发送请求，并使用回调函数处理结果
    auto result_future = flight_mode_client_->async_send_request(request,
        [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture result) {
            if (result.get()->mode_sent)
            {
                RCLCPP_INFO(this->get_logger(), "OFFBOARD mode enabled");
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to enable OFFBOARD mode");
            }
        });
}


void Communication::move_interface_callback(const yolo_detect_message::msg::MoveInterface::SharedPtr msg){
    first_received_controller = true;
  
    if (msg->is_gps == false)
    {
        auto data_message = * msg;
        bool is_vel = data_message.is_vel;
        bool is_body = data_message.is_body;
        bool fixed_high_mode = data_message.fixed_high_mode;
        auto target_vel = std::make_shared<geometry_msgs::msg::Twist>();
        target_vel->linear.x = data_message.vel.linear.x;
        target_vel->linear.y = data_message.vel.linear.y;
        target_vel->linear.z = data_message.vel.linear.z;
        target_vel->angular.x = data_message.vel.angular.x;
        target_vel->angular.y = data_message.vel.angular.y;
        target_vel->angular.z = data_message.vel.angular.z;
        
        // geometry_msgs::msg::Twist target_vel = data_message.vel;
        auto target_body = std::make_shared<geometry_msgs::msg::Pose>();
        target_body->position.x = data_message.pose.position.x;
        target_body->position.y = data_message.pose.position.y;
        target_body->position.z = data_message.pose.position.z;
        target_body->orientation.x = data_message.pose.orientation.x;
        target_body->orientation.y = data_message.pose.orientation.y;
        target_body->orientation.z = data_message.pose.orientation.z;
        target_body->orientation.w = data_message.pose.orientation.w;

        // 定义对应的控制类型
        if(fixed_high_mode == true)
        {
            cmd_vel_fixed_high_callback(target_vel, target_body, is_body);
        }
        else if (is_body == true && is_vel == true)
        {
            cmd_vel_flu_callback(target_vel);
        }else if (is_body == true && is_vel == false){
            cmd_pose_flu_callback(target_body);
        }else if (is_body == false && is_vel == true)
        {
            cmd_vel_enu_callback(target_vel);
        }else if(is_body == false && is_vel == false){
            cmd_pose_enu_callback(target_body);
        }
    }
    else if (msg->is_gps == true)
    {
        target_gps_position.header.frame_id = "map";
        target_gps_position.header.stamp = this->get_clock()->now();
        target_gps_position.pose.position.longitude = msg->geo_pose.longtitude;
        target_gps_position.pose.position.latitude = msg->geo_pose.latitude;
        target_gps_position.pose.position.altitude = msg->geo_pose.altitude;
        {
            std::lock_guard<std::mutex> lock(control_type_mutex);
            control_type = true;
        }
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    
}

void Communication::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
    bool status = msg->status;
    {
        std::lock_guard<std::mutex> lock(server_status_mutex_);
        service_status = status;
    }
    // RCLCPP_INFO(this->get_logger(), "Service status changed to %d", service_status);
}

// GPS的位置回调函数
void Communication::gps_callback(const yolo_detect_message::msg::Geolocation::SharedPtr msg){
    {
        std::lock_guard<std::mutex> lock(target_gps_position_mutex_);
        target_gps_position.header.stamp = this->get_clock()->now();
    
        target_gps_position.pose.position.longitude = msg->longtitude;
        target_gps_position.pose.position.latitude = msg->latitude;
        target_gps_position.pose.position.altitude = msg->altitude;
    }
    {
        std::lock_guard<std::mutex> lock(control_type_mutex);
        control_type = true;
        first_received_controller = true;
    }

    {
        std::lock_guard<std::mutex> lock(last_request_mutex_);
        last_request_  =  this->get_clock()->now();
    }
    {
        std::lock_guard<std::mutex> last_cmd_m(last_cmd_mutex);
        last_cmd_ = "OFFBOARD";
    }
}


bool Communication::is_arrival(const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2, double threshold ) {
    // 计算位置的欧几里得距离
    double dx = pose1.pose.position.x - pose2.pose.position.x;
    double dy = pose1.pose.position.y - pose2.pose.position.y;
    double dz = pose1.pose.position.z - pose2.pose.position.z;
    // double distance = std::sqrt(dx * dx + dy * dy + dz * dz);
    double distance = std::sqrt(dx * dx + dy * dy + dz * dz);
    // 判断距离是否小于阈值
    return distance < threshold;
}


void Communication::set_flight_mode(std::string cmd_string)
{
    if (cmd_string.empty())
    {
        RCLCPP_INFO(this->get_logger()," flying mode is empty,please check input params");
        return;
    }
    
    if (!flight_mode_client_->wait_for_service(std::chrono::seconds(1)))
    {
        RCLCPP_ERROR(this->get_logger(), "Set mode service not available");
        return;
    }

    auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    request->custom_mode = cmd_string;

    // 异步发送请求，并使用回调函数处理结果
    auto result_future = flight_mode_client_->async_send_request(request,
        [this,cmd_string](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture result) {
            if (result.get()->mode_sent)
            {
                RCLCPP_INFO(this->get_logger(), "%s mode enabled",cmd_string.c_str());
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to enable %s mode",cmd_string.c_str());
            }
        });
}